#include "ros/ros.h"
#include "eigen_conversions/eigen_msg.h"
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Cholesky>
#include <Eigen/Geometry>
#include <Eigen/LU>



using namespace Eigen;
using namespace std;



void kinfuPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
//void attitudeCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
ros::Publisher vision_pos_pub;



Affine3d pose_g_enu;
Affine3d pose_cam_uav;
Affine3d pose_cam_uav_inv;
Affine3d pose_uavhrd_uav;


int main(int argc, char **argv)
{
	ros::init(argc, argv, "kinfu_pose_sender_node");
	ros::NodeHandle nh;

	ros::Subscriber stdreo_sub = nh.subscribe("/kinfu_slam/tracker_pose", 1, kinfuPoseCallback);
	//ros::Subscriber mav_att_sub = nh.subscribe("/mavros/local_position/pose",10,attitudeCallback);

	vision_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/vision_pose/pose", 10);

/*
	while(ros::ok())
	{
	  geometry_msgs::PoseStamped fake_pose;
	  fake_pose.header.stamp = ros::Time::now();

	  Isometry3d fake_pose_eigen = Isometry3d::Identity();
	  fake_pose_eigen.linear()<<0,1,0,1,0,0,0,0,-1;
	  fake_pose_eigen.translation()<<0,0,2;
	  tf::poseEigenToMsg(fake_pose_eigen, fake_pose.pose);



	  vision_pos_pub.publish(fake_pose);
	  usleep(20*1000);
	  ros::spinOnce();
	}
*/


	Matrix3d att_cam_uav;
	Matrix3d att_g_enu;

	att_g_enu = AngleAxisd (-M_PI/2, Vector3d::UnitX ()).toRotationMatrix();
	att_cam_uav<<0,0,1,-1,0,0,0,-1,0;
	att_cam_uav = att_cam_uav * AngleAxisd (-0.5, Vector3d::UnitX ()).toRotationMatrix();

	//cout<<att_cam_uav<<endl;

	pose_g_enu.linear() = att_g_enu;
	pose_g_enu.translation() = Vector3d::Zero();

	pose_cam_uav.linear() = att_cam_uav;
	pose_cam_uav.translation() = Vector3d::Zero();
	pose_cam_uav_inv = pose_cam_uav.inverse();

	pose_uavhrd_uav.linear()<<1,0,0,0,-1,0,0,0,-1;
	pose_uavhrd_uav.translation() = Vector3d::Zero();

	ros::spin();
	return 0;

}

/*
bool tmp_init = false;
geometry_msgs::PoseStamped new_msg;
void attitudeCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{

	if(!tmp_init)
	{
		Affine3d pose_uav_enu;
		tf::poseMsgToEigen(msg->pose,pose_uav_enu);
		Affine3d pose_uavhrd_uav;
		pose_uavhrd_uav.linear()<<1,0,0,0,-1,0,0,0,-1;
		pose_uavhrd_uav.translation() = Vector3d::Zero();
		tf::poseEigenToMsg(pose_uav_enu*pose_uavhrd_uav,new_msg.pose);

		tmp_init=true;
	}
	ROS_INFO("AAA");
	new_msg.header.stamp = ros::Time::now();
	vision_pos_pub.publish(new_msg);
}
*/


unsigned long long frameSeq = 0;
Affine3d pose_cam_g;
Affine3d pose_uavhrd_enu;

//PosVelEstimator posvelEstimator;
bool inited = false;
Vector3d position_prev;
bool lockdown = false;
void kinfuPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	if(lockdown)
	{
		ROS_ERROR("LOCKDOWN");
		ros::shutdown();
	}
	geometry_msgs::PoseStamped new_msg;

	new_msg.header = msg->header;


	tf::poseMsgToEigen(msg->pose,pose_cam_g);


	pose_uavhrd_enu = pose_g_enu * pose_cam_g * pose_cam_uav_inv* pose_uavhrd_uav;

/*
	cout<<"*******************************************"<<endl;
	cout<<pose_uav_enu.matrix()<<endl<<endl;
	cout<<"*******************************************"<<endl;
*/
	double pos_diff_abs = 0;
	if(!inited)
	{
		position_prev = pose_uavhrd_enu.translation();
		inited = true;
	}
	else
	{
		Vector3d position_diff = pose_uavhrd_enu.translation()-position_prev;
		pos_diff_abs = position_diff.norm();

		if(pos_diff_abs > 0.20)
		{
			lockdown=true;
			ROS_WARN("[%8.4lf %8.4lf %8.4lf]",pose_uavhrd_enu.translation().x(), pose_uavhrd_enu.translation().y(), pose_uavhrd_enu.translation().z());
			ROS_ERROR("LOCKDOWN %lf",pos_diff_abs);
			ros::shutdown();
		}
		position_prev = pose_uavhrd_enu.translation();
	}


	tf::poseEigenToMsg(pose_uavhrd_enu,new_msg.pose);

/*
	ROS_INFO("**msg_pose_orientation**[%8.4lf %8.4lf %8.4lf %8.4lf]***********",
			msg->pose.orientation.w,msg->pose.orientation.x,msg->pose.orientation.y,msg->pose.orientation.z);
	ROS_INFO("**new_msg_orientation******[%8.4lf %8.4lf %8.4lf %8.4lf]***********",
			new_msg.pose.orientation.w,new_msg.pose.orientation.x,new_msg.pose.orientation.y,new_msg.pose.orientation.z);
*/
	vision_pos_pub.publish(new_msg);
	//ROS_INFO("[%8.4lf %8.4lf %8.4lf %8.4f %8.4f %8.4f]",posvelEstimator.x, posvelEstimator.y, posvelEstimator.z,
						//posvelEstimator.vx, posvelEstimator.vy, posvelEstimator.vz);
	ROS_INFO("[%8.4lf %8.4lf %8.4lf %8.4lf]",new_msg.pose.position.x, new_msg.pose.position.y, new_msg.pose.position.z, pos_diff_abs);

	frameSeq ++;
}


